#include "augmavconn/fifoconnector.h"

/*mTimer * Common_Timer;*/

FiFoConnector::FiFoConnector()
{
    fifoConter=0;
    mapCouter=0;
    AddToTimeMap(HEARTBEAT,1000);
    AddToTimeMap(SYS_STATUS,1000);
    AddToTimeMap(GPS_RAW_INT,1000);
    AddToTimeMap(ATTITUDE,1000);
    AddToTimeMap(GLOBAL_POSITION_INT,200);
    AddToTimeMap(VFR_HUD,200);
    AddToTimeMap(PARAM_VALUE,0);
    AddToTimeMap(MISSION_REQUEST,0);
    AddToTimeMap(MISSION_COUNT,0);
    AddToTimeMap(MISSION_ACK,0);
    AddToTimeMap(MISSION_ITEM,0);
    AddToTimeMap(COMMAND_ACK,0);
    AddToTimeMap(STATE_SP,0);//6.24
    AddToTimeMap(STATUSTEXT,0);

    AddToTimeMap(COMMAND_ACK_CUSTOM,0);
    AddToTimeMap(GLOBAL_POSITION_SYNC,0);
    AddToTimeMap(GLOBAL_POSITION_SYNC_ACK,0);
    AddToTimeMap(AUTOPILOT_VERSION,0);

    bufferType=new BufferType;
    //memset(bufferType.buffer,0,256);
    timer=mTimer::getInstance();

    /*Common_Timer=mTimer::getInstance();*/

    timer->start(Time_100);

}

void
FiFoConnector::addFifo(Message_FIFO_WR_T * fifo)
{
    if(FIFO_count<=MaxFifo)
    {
        fifoList[fifoConter++]=fifo;
    }

}
void

FiFoConnector::ArgMavConn(mavlink_message_t msg , int msgId,Mes_Time message_Time)
{
   //printf("ArgMavConn-1\n");
   Message_FIFO_WR_T * fifo=getFifo(msgId);
    //printf("ArgMavConn-2\n");
   fifo->message_len=msg.len+8;

   if(fifo->getInterval()==0)
   {
       Message_FIFO_WR_T_Aperiodic* fifo_a=    static_cast <Message_FIFO_WR_T_Aperiodic*>(fifo);
       fifo_a->WriteFIFO(msg);
   }
   else
   {
       fifo->WriteFIFO(msg);
       if(fifo->mes_tcount!=BUFSIZE)
       {
            fifo->message_time[fifo->mes_tcount].second=message_Time.second;
            fifo->message_time[fifo->mes_tcount].msec=message_Time.msec;
            //printf("fifo wr :msgid:%d,time: %d:%d\n",msg.msgid,fifo->message_time[0].second,fifo->message_time[0].msec);
            fifo->mes_tcount++;
        }
        else
        {
             fifo->mes_tcount--;
             for(int i=0;i<(BUFSIZE-1);i++)
             {
                fifo->message_time[i].second=fifo->message_time[i+1].second;
                fifo->message_time[i].msec=fifo->message_time[i+1].msec;
              }
              fifo->message_time[fifo->mes_tcount].second=message_Time.second;
              fifo->message_time[fifo->mes_tcount].msec=message_Time.msec;
              fifo->mes_tcount++;
         }
    //printf("ArgMavConn-3");
    }
}

Message_FIFO_WR_T * FiFoConnector::getFifo(int msgID)
{
    int i;
    for(i=0;i<fifoConter;i++)
    {
        if(  fifoList[i]->getID()==msgID)
                {
                    return fifoList[i];
                }
    }
    ///创建新的fifo对应相应ID
    int interval= getInerval(msgID);
    if(interval<0)
    {
        return NULL;
    }
    if(interval==0)
    {
        Message_FIFO_WR_T_Aperiodic* fifo=new Message_FIFO_WR_T_Aperiodic;
        fifo->Initial(interval,msgID);
        fifo->pBuffer=bufferType;
        fifo->Timer=timer;
        fifo->connect();
        addFifo(fifo);
        return fifo;
    }
    else
    {
        Message_FIFO_WR_T* fifo_t=new Message_FIFO_WR_T;
        fifo_t->Initial(interval,msgID);
        fifo_t->pBuffer=bufferType;
        fifo_t->Timer=timer;
        fifo_t->set();//connect Time,and clear every FIFO's Time_count
        fifo_t->connect();
        addFifo(fifo_t);
        return fifo_t;
    }

}
void
FiFoConnector::AddToTimeMap(int msgID, int interval)
{
    if(mapCouter<=MaxMap)
    {
            Imap[mapCouter].interval=interval;
            Imap[mapCouter].msgID=msgID;
            mapCouter++;
    }
}

int FiFoConnector::getInerval(int   msgID)
{
    int i;
    for(i=0;i<mapCouter;i++)
    {
        if(Imap[i].msgID==msgID)
        {
            return Imap[i].interval;
        }
    }
    return -1;
}

FiFoConnector::~FiFoConnector()
{
    if(bufferType!=NULL)
    {
        delete bufferType;
    }

    //int i;
    //for(i=0;i<fifoConter;i++)
    //{

   // }
}
